#include "CSceneNodeAnimatorNewton.h"

namespace irr
{
namespace scene
{

//declare static data members
NewtonWorld* CSceneNodeAnimatorNewton::nWorld = NewtonCreate(NULL, NULL);
core::list<CSceneNodeAnimatorNewton*> CSceneNodeAnimatorNewton::CreatedAnimators;
f32 CSceneNodeAnimatorNewton::CurrentTime = 0;
f32 CSceneNodeAnimatorNewton::TimeAccumulator = 0;
f32 CSceneNodeAnimatorNewton::dt = 10;
core::vector3df CSceneNodeAnimatorNewton::Gravity = core::vector3df(0,-98.f,0);

//! Constructor
CSceneNodeAnimatorNewton::CSceneNodeAnimatorNewton(E_NEWTON_BODY_TYPE type,
                                                   const core::vector3df& size, f32 mass)
    : Mass(mass), Size(size), Type(type), Body(0), FirstAnimate(true), IAmUpdating(false)
{

    //search though the other created animators to see if anyone is updating the newton world
    bool anotherIsUpdating = false;
    core::list<CSceneNodeAnimatorNewton*>::Iterator it = CreatedAnimators.begin();
    for (; it != CreatedAnimators.end(); ++it)
    {
        //if an object is found
        if((*it))
        {
            //is it updating
            if((*it)->IAmUpdating)
                anotherIsUpdating = true;
        }
    }

    //if no other animator is updating the newton world - make this one do it
    if(!anotherIsUpdating)
    {
        IAmUpdating = true;
    }

    //put this animator in the static list
    CreatedAnimators.push_back(this);

    /* the newton world could have been deleted, if at some time in the code no animators where active.
    Because of this the animator needs to check if the newton world is there, and create a new one if it isn't */
    if(!nWorld)
        nWorld = NewtonCreate(NULL, NULL);

    //create the newton body
    NewtonCollision* collision = 0;
    float I[3];

    if(Type == ENBT_CUBE)
    {
        //calculate the inertia
        I[0] = 0.0833f*Mass*(Size.Z*Size.Z + Size.Y*Size.Y);
        I[1] = 0.0833f*Mass*(Size.Z*Size.Z + Size.X*Size.X);
        I[2] = 0.0833f*Mass*(Size.X*Size.X + Size.Y*Size.Y);
        //create the collision
        collision = NewtonCreateBox(nWorld, Size.X, Size.Y, Size.Z, NULL);
    }

    if(Type == ENBT_SPHERE)
    {
        //calculate inertia
        I[0] = 0.2f*Mass*(Size.Z*Size.Z + Size.Y*Size.Y);
        I[1] = 0.2f*Mass*(Size.Z*Size.Z + Size.X*Size.X);
        I[2] = 0.2f*Mass*(Size.X*Size.X + Size.Y*Size.Y);

        //create the collision
        collision = NewtonCreateSphere(nWorld, Size.X, Size.Y, Size.Z, NULL);
    }

    if(collision)
    {
        //create the body
        Body = NewtonCreateBody(nWorld, collision);
        //release the collision
        NewtonReleaseCollision(nWorld, collision);
    }

    if(Body)
    {
        //set the mass and inertia of the body
        NewtonBodySetMassMatrix(Body, Mass, I[0], I[1], I[2]);
        //set the user data of the body
        NewtonBodySetUserData(Body, &BodyUserData);
        //set the callbacks
        NewtonBodySetForceAndTorqueCallback(Body, forceAndTorqueCallback);
        NewtonBodySetTransformCallback(Body, transformCallback);
    }

}

//! destructor
CSceneNodeAnimatorNewton::~CSceneNodeAnimatorNewton()
{

    //a bool to check if other animators is still there
    bool foundOther = false;
    //bool to check if another animator is set to update
    bool anotherIsUpdating = false;

    //iterate though the list to remove the pointer to this animator, and check if other animators are still active
    core::list<CSceneNodeAnimatorNewton*>::Iterator it = CreatedAnimators.begin();
    for (; it != CreatedAnimators.end(); ++it)
    {
        //remove the pointer to this, from the list
        if((*it) == this)
            (*it) = 0;

        //if an animator is found
        if((*it))
            foundOther = true;
    }

    //if this animator was the one updating the newton world - make another one update
    if(IAmUpdating)
    {
        it = CreatedAnimators.begin();
        for(; it!= CreatedAnimators.end(); ++it)
        {
            //if an animator is found
            if((*it))
            {
                //and it is the first one found
                if(!anotherIsUpdating)
                {
                    //make it update
                    (*it)->IAmUpdating = true;
                    anotherIsUpdating = true;
                }
            }
        }
    }

    //delete the newton body
    if(Body)
        NewtonDestroyBody(nWorld, Body);

    //if this is the last animator - delete the newton world
    if(!foundOther)
    {
        if(nWorld)
        {
            NewtonDestroy(nWorld);
            nWorld = 0;
        }
    }

}

//! animates a scene node
void CSceneNodeAnimatorNewton::animateNode(ISceneNode* node, u32 timeMs)
{
    //if this is the first update
    if(FirstAnimate)
    {
        if(Body)
        {
            //apply the node's matrix to the body
            NewtonBodySetMatrix(Body, node->getRelativeTransformation().pointer());
            //put the node pointer in the BodyUserData
            BodyUserData.SceneNode = node;
        }
        FirstAnimate = false;
    }

    //if a scene node other that the original is using the animator
    if(!(node == BodyUserData.SceneNode))
    {
        printf("The Newton animator can only be added to one scene node\n");
        node->removeAnimator(this);
        return;
    }

    //if this animator is taking care of updating the newton world
    if(IAmUpdating)
    {
        //update the newton world
        if(TimeAccumulator < dt)
        {
            //get the time
            f32 newTime = timeMs;
            //calculate the time step
            f32 deltaTime = newTime - CurrentTime;
            //update CurrentTime
            CurrentTime = newTime;
            //add the time step to the time accumulator
            TimeAccumulator += deltaTime;
        }
        //update if the TimeAccumulator is big enough
        if(TimeAccumulator >= dt)
        {
            //update
            NewtonUpdate(nWorld, dt/1000); //convert to seconds
            TimeAccumulator -= dt;
        }
    }
}

void CSceneNodeAnimatorNewton::forceAndTorqueCallback(const NewtonBody* body)
{
   //get the user data
   SBodyUserData* ud = (SBodyUserData*)NewtonBodyGetUserData(body);

   //some floats to hold the information of the body
   float Ixx;
   float Iyy;
   float Izz;
   float mass;
   //get the info and put it in the above floats
   NewtonBodyGetMassMatrix (body, &mass, &Ixx, &Iyy, &Izz);
   //create a force vector with gravity
   float force[3] = {mass*Gravity.X, mass*Gravity.Y, mass*Gravity.Z};
   //apply the force
   NewtonBodySetForce(body, force);
   //add the force and torque of the user data
   NewtonBodyAddForce(body, ud->Force);
   NewtonBodyAddTorque(body, ud->Torque);
   //reset the force and torque vectors
   ud->Force[0] = 0; ud->Force[1] = 0; ud->Force[2] = 0;
   ud->Torque[0] = 0; ud->Torque[1] = 0; ud->Torque[2] = 0;
}

void CSceneNodeAnimatorNewton::transformCallback(const NewtonBody* body, const f32* matrix)
{
    //get the user data
    SBodyUserData* ud = (SBodyUserData*)NewtonBodyGetUserData(body);
    //get the scene node
    ISceneNode* node = ud->SceneNode;

    //make a matrix to hold the body matrix
    core::matrix4 irrMat;
    //copy the body matrix to the matrix4
    memcpy(irrMat.pointer(), matrix, sizeof(float)*16);
    //move and rotate the node according to the matrix
    if(node)
    {
        node->setPosition(irrMat.getTranslation());
        node->setRotation(irrMat.getRotationDegrees());
    }
}

}//end namespace scene
}//end namespace irr
